/**
@file
@brief Modbus server (application layer)
*/
#include "board.h"
#include "MotorControl.h"
#include "mb.h"

#ifndef NULLPTR
#define NULLPTR     0
#endif  // NULLPTR


/* ---------------------------------------------*/
/* ----------- Private Types ------------------ */
/* ---------------------------------------------*/
/**< Memory map from internal memory to modbus address*/
typedef struct {
    // modbus address
    const uint16_t  u16MbAddrStart;       // modbus address of the first holding register
//    uint16_t  u16MbAddrEnd;       // modbus address of the last holding register
    
    // internal memory
    uint8_t*  const pu8MemAddrStart;      // start address of internal memory
    uint16_t  const u16NBytes;            // length of this memrory block
    volatile bool* pbParamChanged;           // pointer to the ParamChanged member of startcture
} MBMemMap_TypeDef;


/* ---------------------------------------------*/
/* ------------ Test Datas -------------------- */
/* ---------------------------------------------*/
struct {
    int16_t rw1_1;
    int16_t rw1_2;
} TestRW_1 = {
    0x11, 0x12
};


struct {
    int16_t rw2_1;
    int16_t rw2_2;
    int16_t rw2_3;
} TestRW_2 = {
    0x21, 0x22, 0x23
};


struct {
    int16_t rw3_1;
} TestRW_3 = {0x31};


volatile bool bTestParamChanged = false;


/* ---------------------------------------------*/
/* -------- Modbus Memory Config -------------- */
/* ---------------------------------------------*/
/**< 
@brief A list of memory map from modbus address (holding registers) to internal memory. 
@note Keep in mind that place struct with lower modbus start address at front.
*/
static MBMemMap_TypeDef MBMemMapLstRW[] = {
    /* Motor 1 paramterts */
    {0x1200U, (uint8_t*)&(M1.ParamRO), sizeof(M1.ParamRO) + sizeof(M1.ParamRW), &(M1.ParamChanged)},  // M1 Motor Control Parameters
    {0x1210U, (uint8_t*)&(hstHBdg_1.Param), sizeof(hstHBdg_1.Param), &(hstHBdg_1.ParamChanged)},
    {0x1220U, (uint8_t*)&(hstEnc_1.Param), sizeof(hstEnc_1.Param), &(hstEnc_1.ParamChanged)},
    {0x1230U, (uint8_t*)&(M1.stPidSpd), sizeof(M1.stPidSpd), NULLPTR},
    {0x1300U, (uint8_t*)&TestRW_1, sizeof(TestRW_1), &bTestParamChanged}, 
    {0x1302U, (uint8_t*)&TestRW_2, sizeof(TestRW_2), &bTestParamChanged},
    {0x1310U, (uint8_t*)&TestRW_3, sizeof(TestRW_3), &bTestParamChanged},
    
    /* Motor 2 paramterts */
    {0x1400U, (uint8_t*)&(M2.ParamRO), sizeof(M2.ParamRO) + sizeof(M2.ParamRW), &(M2.ParamChanged)},  // M2 Motor Control Parameters
    {0x1410U, (uint8_t*)&(hstHBdg_2.Param), sizeof(hstHBdg_2.Param), &(hstHBdg_2.ParamChanged)},
    {0x1430U, (uint8_t*)&(M2.stPidSpd), sizeof(M2.stPidSpd), NULLPTR},
    
    /* Motor 3 paramterts */
    {0x1600U, (uint8_t*)&(M3.ParamRO), sizeof(M3.ParamRO) + sizeof(M3.ParamRW), &(M3.ParamChanged)},  // M3 Motor Control Parameters
    {0x1610U, (uint8_t*)&(hstHBdg_3.Param), sizeof(hstHBdg_3.Param), &(hstHBdg_3.ParamChanged)},
    {0x1630U, (uint8_t*)&(M3.stPidSpd), sizeof(M3.stPidSpd), NULLPTR},
    
    /* Motor 4 paramterts */
    {0x1800U, (uint8_t*)&(M4.ParamRO), sizeof(M4.ParamRO) + sizeof(M4.ParamRW), &(M4.ParamChanged)},  // M4 Motor Control Parameters
    {0x1810U, (uint8_t*)&(hstHBdg_4.Param), sizeof(hstHBdg_4.Param), &(hstHBdg_4.ParamChanged)},
    {0x1830U, (uint8_t*)&(M4.stPidSpd), sizeof(M4.stPidSpd), NULLPTR},
};

#define MEM_MAP_LEN  (sizeof(MBMemMapLstRW) / sizeof(MBMemMap_TypeDef))


/* ---------------------------------------------*/
/* ----------- Private Functions -------------- */
/* ---------------------------------------------*/
//static eMBErrorCode 
//MBMemBoundaryCheck(const MBMemMap_TypeDef* const mem, uint16_t u16MbAddrStart, uint16_t u16NRegs) {
//    uint16_t _offset = u16MbAddrStart - (mem->u16MbAddrStart);
//    // the remaining length of this memory block at given modbus start address, in the unit of holding register
//    uint16_t _remaining_n_regs = (mem->u16NBytes) / 2 - _offset;
//    
//    if (u16NRegs <= _remaining_n_regs) {
//        return MB_ENOERR;
//    }
//    else {
//        return MB_ENOREG;
//    }
//}


/**
@brief Check if the requested modbus address and quantities are available.
*/
static eMBErrorCode
MBCheckAddr(uint16_t u16MbAddrStart, uint16_t u16NRegs) {
    MBMemMap_TypeDef* _mem;
    uint16_t _reg_offset;
    uint16_t _n_availalbe_regs = 0;
    
    int16_t i;
    // === search the first addr of memory less than u16MbAddrStart ===
    for (i=MEM_MAP_LEN-1; i>0; i--) {
        if (u16MbAddrStart >= MBMemMapLstRW[i].u16MbAddrStart) {
            break;
        }
    }
    if (i < 0) {
        return MB_ENOREG;  // address no found
    }
    
    // === is current memory block has enough registers? ===
    _mem = &(MBMemMapLstRW[i]);
    _reg_offset = u16MbAddrStart - (*_mem).u16MbAddrStart;
    _n_availalbe_regs = (*_mem).u16NBytes/2 - _reg_offset;
    
    if (u16NRegs <= _n_availalbe_regs) {
        return MB_ENOERR;  // this memory block has enough registers
    }
    
    // === is following memory blocks are contiguous? ===
    // === if so, are they has enough registers? ===
    i++;
    for (; i<MEM_MAP_LEN; i++) {
        _mem = &(MBMemMapLstRW[i]);
        
        if (! ((u16MbAddrStart + _n_availalbe_regs) == (_mem->u16MbAddrStart)) ) {
            return MB_ENOREG;  // this memory block is not contiguous with the former one
        }
        
        _n_availalbe_regs += _mem->u16NBytes / 2;
        if (u16NRegs <= _n_availalbe_regs) {
            return MB_ENOERR;  // the contiguous memory block has enough registers
        }
    }
    
    return MB_ENOREG;  // no enough registers
}


/**@note Assume all parameters are checked and correct. */
static void MBHodingMemRead(uint8_t * pucRegBuffer, uint16_t u16Address, uint16_t u16NRegs) {
    MBMemMap_TypeDef* _mem;
    uint16_t _offset;
    uint16_t _n_regs_avail;
    uint16_t _n_regs_to_read;       // how many regs are behind of position from offset
    uint8_t* pu8DatBase;            // data base address
    
    // === search the first address less than usAddress ===
    int16_t i;
    for (i=MEM_MAP_LEN-1; i>0; i--) {
        if (u16Address >= MBMemMapLstRW[i].u16MbAddrStart) {
            break;
        }
    }
    
    // === read data ===
    // read from first memo block
    _mem = &(MBMemMapLstRW[i]);
    _offset = u16Address - (*_mem).u16MbAddrStart;
    _n_regs_avail = (*_mem).u16NBytes / 2 - _offset;  // how many regs are behind from offset
    pu8DatBase = (*_mem).pu8MemAddrStart + _offset*2;
    
    _n_regs_to_read = (_n_regs_avail > u16NRegs) ? u16NRegs : _n_regs_avail;
    u16NRegs -= _n_regs_to_read;
    
    for (int j=0; j<_n_regs_to_read; j++) {
        // write buffer in big endian
        pucRegBuffer[j*2] = pu8DatBase[j*2 + 1];
        pucRegBuffer[j*2 + 1] = pu8DatBase[j*2];
    }
    pucRegBuffer += _n_regs_to_read *2;
    
    // read from the following blocks
    while (u16NRegs > 0) {
        i++;
        _mem = &(MBMemMapLstRW[i]);
        pu8DatBase = (*_mem).pu8MemAddrStart;
        _n_regs_avail = (*_mem).u16NBytes / 2;
        
        _n_regs_to_read = (_n_regs_avail > u16NRegs) ? u16NRegs : _n_regs_avail;
        u16NRegs -= _n_regs_to_read;
        
        for (int j=0; j<_n_regs_to_read; j++) {
            // write buffer in big endian
            pucRegBuffer[j*2] = pu8DatBase[j*2 + 1];
            pucRegBuffer[j*2 + 1] = pu8DatBase[j*2];
        }
        pucRegBuffer += _n_regs_to_read *2;
    }
}


/**@note Assume all parameters are checked and correct. */
static void MBHodingMemWrite(uint8_t * pucRegBuffer, uint16_t u16Address, uint16_t u16NRegs) {
    MBMemMap_TypeDef* _mem;
    uint16_t _offset;
    uint16_t _n_regs_avail;
    uint16_t _n_regs_to_write;      // how many regs are behind of position from offset
    uint8_t* pu8DatBase;            // data base address
    
    // === search the first address less than usAddress ===
    int16_t i;
    for (i=MEM_MAP_LEN-1; i>0; i--) {
        if (u16Address >= MBMemMapLstRW[i].u16MbAddrStart) {
            break;
        }
    }
    
    // === read data ===
    // read from first memo block
    _mem = &(MBMemMapLstRW[i]);
    _offset = u16Address - (*_mem).u16MbAddrStart;
    _n_regs_avail = (*_mem).u16NBytes / 2 - _offset;  // how many regs are behind from offset
    pu8DatBase = (*_mem).pu8MemAddrStart + _offset*2;
    
    _n_regs_to_write = (_n_regs_avail > u16NRegs) ? u16NRegs : _n_regs_avail;
    u16NRegs -= _n_regs_to_write;
    
    for (int j=0; j<_n_regs_to_write; j++) {
        // write buffer in big endian
        pu8DatBase[j*2] = pucRegBuffer[j*2 + 1];
        pu8DatBase[j*2 + 1] = pucRegBuffer[j*2];
    }
    pucRegBuffer += _n_regs_to_write *2;
    if (NULLPTR != _mem->pbParamChanged) {
        *(_mem->pbParamChanged) = true;
    }
    
    // read from the following blocks
    while (u16NRegs > 0) {
        i++;
        _mem = &(MBMemMapLstRW[i]);
        pu8DatBase = (*_mem).pu8MemAddrStart;
        _n_regs_avail = (*_mem).u16NBytes / 2;
        
        _n_regs_to_write = (_n_regs_avail > u16NRegs) ? u16NRegs : _n_regs_avail;
        u16NRegs -= _n_regs_to_write;
        
        for (int j=0; j<_n_regs_to_write; j++) {
            // write buffer in big endian
            pu8DatBase[j*2] = pucRegBuffer[j*2 + 1];
            pu8DatBase[j*2 + 1] = pucRegBuffer[j*2];
        }
        pucRegBuffer += _n_regs_to_write *2;
        if (NULLPTR != _mem->pbParamChanged) {
            *(_mem->pbParamChanged) = true;
        }
    }
}


/* ---------------------------------------------*/
/* -------- Freemodbus Functions -------------- */
/* ---------------------------------------------*/

eMBErrorCode
eMBRegHoldingCB( UCHAR * pucRegBuffer, USHORT usAddress, USHORT usNRegs, eMBRegisterMode eMode )
{
    eMBErrorCode _err_code = MB_ENOERR;

    usAddress --;  // 1 based addr to 0 based addr
    
    // ===== Check if address is available and quantity of registers are enough =====
    _err_code = MBCheckAddr(usAddress, usNRegs);
    if (MB_ENOERR != _err_code) {
        return _err_code;
    }
    
    // ===== do reading or writing =====

    if (MB_REG_READ == eMode) {
        MBHodingMemRead(pucRegBuffer, usAddress, usNRegs);
    }
    else if (MB_REG_WRITE == eMode) {
        MBHodingMemWrite(pucRegBuffer, usAddress, usNRegs);
    }
    
    return MB_ENOERR;
}

